/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
/*
串口发送数据格式（串口波特率19200，1个启始位，1个停止位，其它无）：
发送间隔40MS，一次发送8个BYTE，
BYTE[0]:数据头，固定为0X66
BYTE[1]:AIL——副翼 ： 中间值0x80，左边最大为0x00，右边最大为0xff，即0x00-0x80-0xff线性变化
BYTE[2]:ELE——升降舵：中间值0x80，后最大为0x00，前最大为0xff，即0x00-0x80-0xff线性变化
BYTE[3]:THR——油门：0x00为最小，0xff为最大
BYTE[4]:RUDD——方向舵  中间值0x80，左转最大为0x00，右转最大为0xff，即0x00-0x80-0xff线性变化
BYTE[5]:标志位
BYTE[6]=(BYTE[1]^BYTE[2]^BYTE[3]^BYTE[4]^BYTE[5])&0xff;
BYTE[7]：数据尾，固定为0x99
BYTE[5]

bit0=一键起飞    先置1，1秒后置0
bit1=一键下降    先置1，1秒后置0
bit2=紧急停止   先置1，1秒后置0
bit3=360度翻转   当翻滚按键按下，并且方向键移动超过一半，发送1，并且方向值发最大值 持续时间大概300毫秒,例如前翻 BYTE[2］=0x0ff
                              （此设计主要跟速度档位有高低中速度有关）
bit4=1 无头模式   0有头模式
bit5=1 一键返航   按下置1，弹起置0
bit6=1 灯光       按下置1，弹起置0，初始化为1
bit7=飞控陀螺仪矫正   先置1，1秒后置0

*/
#include "camera_rc.h"
#include <stdbool.h>
#include <string.h>
#include "uart.h"
#define CAM_VER 0

static uint8_t camera_rc_data[CAMERA_RC_COUNT];
static uint8_t camera_send_back_data[32];
static uint8_t rssi;
int16_t cameraChannel[4];

/**
 * @brief  init and check camera rc
 * @note
 * @param  *UASRTX: which serial is camera attach
 * @retval 0: suceed 1: no data 2: wrong data 3: check fail
 */

/**
 * @brief  update camera rc data
 * @note
 * @retval 0: suceed 2: wrong data 3: check fail
 */
uint8_t camera_rc_update(uint8_t *DataFlow)
{

    usartPrintf("\n rssi:%d:", camera_rc_data[CAMERA_RSSI]);

#if CAM_VER == 1
    usartPrintf("110\n");

    if ((DataFlow[0] == CAMERA_RC_HEADER) && (DataFlow[63] == CAMERA_RC_TAIL))
    {
        uint8_t tempCheck = DataFlow[1];
        for (uint8_t i = 2; i <= 61; i++)
        {
            tempCheck = dataCheck(tempCheck, DataFlow[i]);
        }
        tempCheck = tempCheck & 0xff;
        camera_rc_data[CAMERA_CHECK] = tempCheck;
        if (camera_rc_data[CAMERA_CHECK] == DataFlow[62])
        {
            camera_rc_data[CAMERA_ROLL] = DataFlow[7];
            camera_rc_data[CAMERA_PITCH] = DataFlow[8];
            camera_rc_data[CAMERA_THROTTLE] = DataFlow[9];
            camera_rc_data[CAMERA_YAW] = DataFlow[10];
            camera_rc_data[CAMREA_FLAG_ONE] = DataFlow[15];
            camera_rc_data[CAMREA_FLAG_TWO] = DataFlow[16];
            rssi++;
        }
        else
        {
            // usartPrintf("wrong crc\n");
            return 3; // check error
        }
    }
    else
    {
        // usartPrintf("wrong msg\n");
        return 2; // wrong data
    }

#elif CAM_VER == 0

    camera_rc_data[CAMERA_CHECK] = (DataFlow[1] ^ DataFlow[2] ^ DataFlow[3] ^ DataFlow[4] ^ DataFlow[5]) & 0xff;
    if (camera_rc_data[CAMERA_CHECK] == DataFlow[6])
    {

        camera_rc_data[CAMERA_HEADER] = DataFlow[0];
        camera_rc_data[CAMERA_ROLL] = DataFlow[1];
        camera_rc_data[CAMERA_PITCH] = DataFlow[2];
        camera_rc_data[CAMERA_THROTTLE] = DataFlow[3];

        camera_rc_data[CAMERA_YAW] = DataFlow[4];
        camera_rc_data[CAMREA_FLAG_ONE] = DataFlow[5];
        camera_rc_data[CAMERA_CHECK] = DataFlow[6];
        camera_rc_data[CAMERA_TAIL] = DataFlow[7];

        rssi++;
    }
    else
    {
        // usartPrintf("wrong crc\n");
        return 3; // check error
    }

#endif

    return 0;
}

uint8_t dataCheck(uint8_t a, uint8_t b)
{
    uint8_t result = 0;
    result = (uint8_t)(a ^ b);
    return result;
}

/**
 * @brief  get camera rc channel data
 * @note
 * @param  index:   CAMERA_ROLL,//BYTE[1]:AIL——副翼 ： 中间值0x80，左边最大为0x00，右边最大为0xff，即0x00-0x80-0xff线性变化;
                    CAMERA_PITCH,//BYTE[2]:ELE——升降舵：中间值0x80，后最大为0x00，前最大为0xff，即0x00-0x80-0xff线性变化;
                    CAMERA_THROTTLE,//BYTE[3]:THR——油门：0x00为最小，0xff为最大;
                    CAMERA_YAW,//BYTE[4]:RUDD——方向舵  中间值0x80，左转最大为0x00，右转最大为0xff，即0x00-0x80-0xff线性变化;
                    CAMERA_RSSI,//信号个数，每秒更新;
 * @retval data of index
 */
uint8_t get_camera_rc_data(uint8_t index)
{
    return camera_rc_data[index];
}

/**
 * @brief  check flag status
 * @note
 * @param  flag:
 *              CAMERA_TAKEOFF = 1<<0,//bit0 一键起飞    先置1，1秒后置0
                CAMERA_LAND = 1<<1,//bit1 一键下降    先置1，1秒后置0
                CAMERA_STOP = 1<<2,//bit2 紧急停止   先置1，1秒后置0
                CAMERA_ROLLING_OVER = 1<<3,//bit3 360度翻转   当翻滚按键按下，并且方向键移动超过一半，发送1，并且方向值发最大值 持续时间大概300毫秒,例如前翻 BYTE[2］=0x0ff,（此设计主要跟速度档位有高低中速度有关）
                CAMERA_HEADLESS= 1<<4,//bit4 1 无头模式   0有头模式
                CAMERA_CLI=1<<7,//bit7 飞控陀螺仪矫正   先置1，1秒后置0
 * @retval
 */

uint8_t check_camera_rc_flag(uint16_t flag)
{

    uint8_t ret=0;

    if (flag > 15)
    {
        ret= camera_rc_data[CAMREA_FLAG_THREE] & (1 << (flag - 15));

    }
    else if (flag > 7)
    {
        ret=  (camera_rc_data[CAMREA_FLAG_TWO] & (1 << (flag - 8)));
    }
    else
    {
        ret = (camera_rc_data[CAMREA_FLAG_ONE] & 1 << flag);
    }

    if(ret>0){
        return 1;
    }
}

void CameraRcRemap(void)
{
    if (get_camera_rc_data(CAMERA_RSSI))
    {
        // usartPrintf("ttttttttttttttttttttt\n");

        int16_t camThrottle = get_camera_rc_data(CAMERA_THROTTLE);
        int16_t camRoll = get_camera_rc_data(CAMERA_ROLL);
        int16_t camYaw = get_camera_rc_data(CAMERA_YAW);

        camThrottle = applyDeadband((camThrottle - 128), 2) * 7;
        camRoll = applyDeadband((camRoll - 128), 2) * 7;
        camYaw = applyDeadband((camYaw - 128), 2) * 7;

        if (get_rc_fps() == 0)
        {
            car_cmd_channel[CMD_THROTTLE] = camThrottle;
            car_cmd_channel[CMD_TURN] = camRoll;
            car_cmd_channel[CMD_SHIFT] = camYaw;
        }
        else
        {
            if (!rcChannel[RC_THROTTLE])
                car_cmd_channel[CMD_THROTTLE] = camThrottle;
            if (!rcChannel[RC_ROLL])
                car_cmd_channel[CMD_TURN] = camRoll;
            if (!rcChannel[RC_YAW])
                car_cmd_channel[CMD_SHIFT] = camYaw;
        }
        //  usartPrintf("RSSI:%d %d %d %d\n", camera_rc_data[CAMERA_RSSI], camThrottle, camRoll, camYaw);
    }
}



//检查摄像头摇器空闲，非空闲时阻止系统关机,1空闲，0非闲
bool CameraRcIdleDetect(void)
{
    static uint8_t camRcRecord[CAMERA_RC_COUNT - 1];
    uint16_t bTmp = 0;
    for (uint8_t i = CAMERA_ROLL; i < CAMERA_CHECK; i++)
    {
        if (camRcRecord[i] != camera_rc_data[i])
        {
            camRcRecord[i] = camera_rc_data[i];
            bTmp |= (1 << i);
            // usartPrintf("camera_rc_data[%d]:%d\n",i,camera_rc_data[i]);
        }
        else
        {
            bTmp &= ~(1 << i);
        }
    }

    // static uint32_t secondTmp = 0;
    // if (millis() > (secondTmp + 1000))
    // {
    //     secondTmp = millis();
    //     camera_rc_data[CAMERA_RSSI] = rssi;
    //     rssi = 0;
    // }

    if (bTmp)
    {
        // resetSystemShutdownCountdown();
        return 0;
    }
    return 1;
}
static uint32_t usingBaudrate = 38400;
static uint32_t usedBaudrate = 0;
static uint32_t preMs = 0;
static uint8_t LostCnt = 0;
void cameraRcRxHandle(void)
{
    // if(IsSerialRxHasData(&serialHandle[serial4]))
    //     usartPrintf("Check Camera\n");
    // else
    //     usartPrintf("No data\n");
    if (usedBaudrate != usingBaudrate)
    {
        usedBaudrate = usingBaudrate;
        // Serial_Begin(&serialHandle[serial4], usedBaudrate);
        // Serial_Pruge(&serialHandle[serial4]);
        // if (usedBaudrate != 19200 || usedBaudrate != 38400)
        // {
        //     usartPrintf("***********************uart4 reInit to:%d\n", usedBaudrate);
        // }
        // usartPrintf("uart4 reInit to:%d\n", usedBaudrate);
    }
    if (millis() > (preMs + 1000))
    {
        preMs = millis();
        camera_rc_data[CAMERA_RSSI] = rssi;
        if (camera_rc_data[CAMERA_RSSI] == 0) //没信号时切换波特率
        {
            if (LostCnt++ > 3)
            {
                LostCnt = 0;
                if (usedBaudrate == 19200)
                {
                    usingBaudrate = 38400;
                }
                else
                {
                    usingBaudrate = 19200;
                }
            }
        }
        rssi = 0;
    }
    uint8_t rxBytesCount = IsSerialRxHasData(&serialHandle[serial4]);
    // uint8_t *DataFlow = (uint8_t *)Serial_gets(&serialHandle[serial4]);
    if (rxBytesCount == 8) //串口频率为19200是旧版的，8个节
    {
        if (usedBaudrate == 19200) //波特率匹配
        {
            camera_rc_data[CAMERA_CHECK] = (DataFlow[1] ^ DataFlow[2] ^ DataFlow[3] ^ DataFlow[4] ^ DataFlow[5]) & 0xff;
            if (camera_rc_data[CAMERA_CHECK] == DataFlow[6]) //校验
            {
                camera_rc_data[CAMERA_HEADER] = DataFlow[0];
                camera_rc_data[CAMERA_ROLL] = DataFlow[1];
                camera_rc_data[CAMERA_PITCH] = DataFlow[2];
                camera_rc_data[CAMERA_THROTTLE] = DataFlow[3];
                camera_rc_data[CAMERA_YAW] = DataFlow[4];
                camera_rc_data[CAMREA_FLAG_ONE] = DataFlow[5];
                camera_rc_data[CAMERA_CHECK] = DataFlow[6];
                camera_rc_data[CAMERA_TAIL] = DataFlow[7];
                rssi++;
            }
        }
    }
    else if (rxBytesCount == 64) //串口频率为38400是新版的，64个节
    {
        if (usedBaudrate == 38400) //波特率匹配
        {
            if ((DataFlow[0] == CAMERA_RC_HEADER) && (DataFlow[63] == CAMERA_RC_TAIL))
            {
                uint8_t tempCheck = DataFlow[1];
                for (uint8_t i = 2; i <= 61; i++)
                {
                    tempCheck ^= DataFlow[i];
                }
                tempCheck = tempCheck & 0xff;
                camera_rc_data[CAMERA_CHECK] = tempCheck;
                //     if(camera_rc_data[CAMERA_CHECK] == DataFlow[62])
                // if (true)
                {
                    camera_rc_data[CAMERA_ROLL] = DataFlow[7];
                    camera_rc_data[CAMERA_PITCH] = DataFlow[8];
                    camera_rc_data[CAMERA_THROTTLE] = DataFlow[9];
                    camera_rc_data[CAMERA_YAW] = DataFlow[10];
                    camera_rc_data[CAMREA_FLAG_ONE] = DataFlow[15];
                    camera_rc_data[CAMREA_FLAG_TWO] = DataFlow[16];
                    camera_rc_data[CAMREA_FLAG_THREE] = DataFlow[17];
                    camera_rc_data[CAMREA_FLAG_FOUR] = DataFlow[18];
                    // LostCnt=0;
                    rssi++;
                }
            }
        }
    }
    if (rxBytesCount > 1)
        Serial_Pruge(&serialHandle[serial4]);
}
void camera_rc_send_back_update(void)
{





    for (int i = 0; i < 32; i++)
    {
        camera_send_back_data[i] = 0;
    }

    uint8_t data3 = (fc_error_flag >> 24) & 0xff;
    uint8_t data2 = (fc_error_flag >> 16) & 0xff;
    uint8_t data1 = (fc_error_flag >> 8) & 0xff;
    uint8_t data0 = fc_error_flag & 0Xff;

    camera_send_back_data[CAMERA_BACK_HEADER] = 0x66;
    camera_send_back_data[CAMERA_BACK_TYPE] = 0x01; //默认使用1，表示回传
    camera_send_back_data[CAMERA_BACK_DATA_TYPE] = 0x00; //暂时没有用， 数据太多时，不够放，可以用个值表示不同组的数据发送
    camera_send_back_data[CAMERA_BACK_MAJOR_VER] = get_dev_major_verion();
    camera_send_back_data[CAMERA_BACK_MINOR_VER] = get_dev_minor_verion();
    camera_send_back_data[CAMERA_BACK_BUILD_NUMBER] = get_dev_build_number();
    camera_send_back_data[CAMERA_BACK_CORE_ID] = get_dev_core_id();
    camera_send_back_data[CAMERA_BACK_DEV_ID] = (uint8_t)get_dev_id();
#ifdef HeartOfMecha
    camera_send_back_data[CAMERA_BACK_EQM_ID] = getTransformState() + 10;  //第一个型号从10开始
#else
    camera_send_back_data[CAMERA_BACK_EQM_ID] = (uint8_t)get_eqm_id();
#endif
    camera_send_back_data[CAMERA_BACK_FC_FLAG_ERROR_3] = data3;
    camera_send_back_data[CAMERA_BACK_FC_FLAG_ERROR_2] = data2;
    camera_send_back_data[CAMERA_BACK_FC_FLAG_ERROR_1] = data1;
    camera_send_back_data[CAMERA_BACK_FC_FLAG_ERROR_0] = data0;
    camera_send_back_data[CAMERA_BACK_TAIL] = 0x99;

    uint8_t tempCheck = camera_send_back_data[1];
    for (uint8_t i = 2; i <= 29; i++)
    {
        tempCheck ^= camera_send_back_data[i];
    }
    tempCheck = tempCheck & 0xff;
    camera_send_back_data[CAMERA_BACK_CHECK] = tempCheck;
    camera_send_back_data[0] = 0x66;
    camera_send_back_data[31] = 0x99;

    for (int i = 0; i < 32; i++)
    {
        Serial_putc(&serialHandle[serial4], camera_send_back_data[i]);
    }

    Serial_flush(&serialHandle[serial4]);


}
